
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       fms_mode.c
  * @author     baiyang
  * @date       2023-7-6
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "fms.h"
#include "logger.h"
#include "quadplane.h"
#include "qautotune.h"

#include <assert.h>

#include <notify/notify.h>
#include <parameter/param.h>
#include <common/time/gp_time.h>
#include <common/gp_math/gp_control.h>
#include <internal_error/internal_error.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static void notify_mode(mode_base_const_t mode);
static void notify_no_such_mode(uint8_t mode_number);
static uint8_t readSwitch(void);
/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
bool fms_set_mode(mode_base_t new_mode, const ModeReason reason)
{
    if (fms.control_mode == new_mode) {
        // don't switch modes if we are already in the correct mode.
        // only make happy noise if using a difent method to switch, this stops beeping for repeated change mode requests from GCS
        if ((reason != fms.control_mode_reason) && (reason != MODE_REASON_INITIALISED)) {
            notify_events.user_mode_change = 1;
        }
        return true;
    }

#if HAL_QUADPLANE_ENABLED
    if (mode_is_vtol_mode(new_mode) && !plane.quadplane.available()) {
        // dont try and switch to a Q mode if quadplane is not enabled and initalized
        gcs_send_text(MAV_SEVERITY_INFO,"Q_ENABLE 0");
        // make sad noise
        if (reason != MODE_REASON_INITIALISED) {
            notify_events.user_mode_change_failed = 1;
        }
        return false;
    }

#else
    if (mode_is_vtol_mode(new_mode)) {
        INTERNAL_ERROR(internal_error_flow_of_control);
        gcs_send_text(MAV_SEVERITY_INFO,"HAL_QUADPLANE_ENABLED=0");
        // make sad noise
        if (reason != MODE_REASON_INITIALISED) {
            notify_events.user_mode_change_failed = 1;
        }
        return false;
    }
#endif  // HAL_QUADPLANE_ENABLED

    // backup current control_mode and previous_mode
    mode_base_t old_previous_mode = fms.previous_mode;
    mode_base_t old_mode = fms.control_mode;

    // update control_mode assuming success
    // TODO: move these to be after enter() once start_command_callback() no longer checks control_mode
    fms.previous_mode = fms.control_mode;
    fms.control_mode = new_mode;
    const ModeReason old_previous_mode_reason = fms.previous_mode_reason;
    fms.previous_mode_reason = fms.control_mode_reason;
    fms.control_mode_reason = reason;

    // attempt to enter new mode
    if (!mode_enter(new_mode)) {
        // Log error that we failed to enter desired flight mode
        gcs_send_text(MAV_SEVERITY_WARNING, "Flight mode change failed");

        // we failed entering new mode, roll back to old
        fms.previous_mode = old_previous_mode;
        fms.control_mode = old_mode;
        fms.control_mode_reason = fms.previous_mode_reason;
        fms.previous_mode_reason = old_previous_mode_reason;

        // make sad noise
        if (reason != MODE_REASON_INITIALISED) {
            notify_events.user_mode_change_failed = 1;
        }
        return false;
    }

    // exit previous mode
    mode_exit(old_mode );

    // log and notify mode change
    aplog_write_mode(mode_number(fms.control_mode), fms.control_mode_reason);
    notify_mode(fms.control_mode);
    gcs_send_message(MAV_MSG_HEARTBEAT);

    // make happy noise
    if (reason != MODE_REASON_INITIALISED) {
        notify_events.user_mode_change = 1;
    }
    return true;
}

bool fms_set_mode2(const uint8_t new_mode, const ModeReason reason)
{
    return fms_set_mode_by_number((enum ModeNumber)new_mode, reason);
}

bool fms_set_mode_by_number(enum ModeNumber new_mode_number, const ModeReason reason)
{
    mode_base_t new_mode = fms_mode_from_mode_num(new_mode_number);
    if (new_mode == NULL) {
        notify_no_such_mode(new_mode_number);
        return false;
    }
    return fms_set_mode(new_mode, reason);
}

mode_base_t fms_mode_from_mode_num(const enum ModeNumber num)
{
    mode_base_t ret = NULL;
    switch (num) {
    case MN_MANUAL:
        ret = (mode_base_t)&fms.mode_manual;
        break;
    case MN_FLY_BY_WIRE_A:
        ret = (mode_base_t)&fms.mode_fbwa;
        break;
    case MN_STABILIZE:
        ret = (mode_base_t)&fms.mode_stabilize;
        break;
    case MN_ACRO:
        ret = (mode_base_t)&fms.mode_acro;
        break;
    default:
        break;
    }
    return ret;
}

/*
  are we flying inverted?
 */
bool fms_fly_inverted(void)
{
    if (fms.control_mode == (mode_base_t)&fms.mode_manual) {
        return false;
    }
    if (fms.inverted_flight) {
        // controlled with aux switch
        return true;
    }
    if (fms.control_mode == (mode_base_t)&fms.mode_auto && fms.auto_state.inverted_flight) {
        return true;
    }
    return false;
}

// check if FLIGHT_OPTION is enabled
bool fms_flight_option_enabled(enum FlightOptions flight_option)
{
    return fms.g.flight_options & flight_option;
}

/*
  main control mode dependent update code
 */
void fms_update_control_mode(void)
{
    if (fms.control_mode != (mode_base_t)&fms.mode_auto) {
        // hold_course is only used in takeoff and landing
        fms.steer_state.hold_course_cd = -1;
    }

    fms_update_fly_forward();

    mode_update(fms.control_mode);
}

void fms_update_fly_forward(void)
{
    ahrs_view* ahrs = get_ahrs_view();

    // ensure we are fly-forward when we are flying as a pure fixed
    // wing aircraft. This helps the EKF produce better state
    // estimates as it can make stronger assumptions
#if HAL_QUADPLANE_ENABLED
    if (quadplane.available() &&
        quadplane.tailsitter.is_in_fw_flight()) {
        ahrs_set_fly_forward(true);
        return;
    }

    if (quadplane.in_vtol_mode() ||
        quadplane.in_assisted_flight()) {
        ahrs_set_fly_forward(false);
        return;
    }
#endif

#if HAL_QUADPLANE_ENABLED
    if (fms.flight_stage == FWFS_LAND) {
        ahrs_set_fly_forward(ahrs, landing.is_flying_forward());
        return;
    }
#endif

    ahrs_set_fly_forward(ahrs, true);
}

static uint8_t readSwitch(void)
{
    uint16_t pulsewidth = rcs_get_radio_in(fms.g.flight_mode_channel - 1);
    if (pulsewidth <= 900 || pulsewidth >= 2200) return 255;            // This is an error condition
    if (pulsewidth <= 1230) return 0;
    if (pulsewidth <= 1360) return 1;
    if (pulsewidth <= 1490) return 2;
    if (pulsewidth <= 1620) return 3;
    if (pulsewidth <= 1749) return 4;              // Software Manual
    return 5;                                                           // Hardware Manual
}

// sets notify object flight mode information
static void notify_mode(mode_base_const_t mode)
{
    notify_flags.flight_mode = mode_number(mode);
    notify_set_flight_mode_str(mode_name4(mode));
}

static void notify_no_such_mode(uint8_t mode_number)
{
    GCS_SEND_TEXT(MAV_SEVERITY_WARNING,"No such mode %u", mode_number);
    //aplog_write_error(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode_number));
}

/*------------------------------------test------------------------------------*/


